/**
 * ****************************************************************************
 * Project: RoboBridge
 * ----------------------------------------------------------------------------
 * -------------------------- [c]2012 - KrawallMann ---------------------------
 * ---------------------- http://www.krawallmann.de.tc/ -----------------------
 * ****************************************************************************
 * File: Test.java Package: tc.de.krawallmann Version: 1.0 Author(s):
 * KrawallMann
 * ****************************************************************************
 * Description:
 * ****************************************************************************
 */
package tc.de.krawallmann;

import java.io.IOException;

import tc.de.krawallmann.robobridge.AbstractRoboBridgeApplication;
import tc.de.krawallmann.robobridge.TimedMultiState;
import tc.de.krawallmann.robobridge.MultiStateListener;
import tc.de.krawallmann.robobridge.RoboBridge;
import tc.de.krawallmann.robobridge.behaviors.BehaviorController;
import tc.de.krawallmann.robobridge.behaviors.CruiseBehavior;
import tc.de.krawallmann.robobridge.behaviors.EscapeBehavior;
import tc.de.krawallmann.robobridge.communication.AbstractRemoteCommunicationDevice;
import tc.de.krawallmann.robobridge.communication.ConnectionListener;
import tc.de.krawallmann.robobridge.communication.lan.wlan.WLANRemoteDevice;
import tc.de.krawallmann.robobridge.devices.AbstractDevice;
import tc.de.krawallmann.robobridge.devices.Robot;
import tc.de.krawallmann.robobridge.devices.SensorListener;
import tc.de.krawallmann.robobridge.devices.implementation.CMPS03;
import tc.de.krawallmann.robobridge.devices.implementation.Direction;
import tc.de.krawallmann.robobridge.devices.implementation.DrivingLights;
import tc.de.krawallmann.robobridge.devices.implementation.LCD03;
import tc.de.krawallmann.robobridge.devices.implementation.M256;
import tc.de.krawallmann.robobridge.devices.implementation.RNControl;
import tc.de.krawallmann.robobridge.devices.implementation.RP6Base;
import tc.de.krawallmann.robobridge.devices.implementation.SRF08;
import tc.de.krawallmann.robobridge.devices.implementation.SRF08OnServo;

/**
 * Basic test.
 * 
 * @author KrawallMann
 * @version 1.0
 */
public class Test extends AbstractRoboBridgeApplication implements SensorListener {

	/**
	 * The main method.
	 * 
	 * @param args
	 *            the arguments
	 */
	public static void main(final String[] args) {
		try {
			new Test();
		} catch (final IOException e) {
			e.printStackTrace();
		}
	}

	/** The base. */
	private transient RP6Base base;

	/** The cmps. */
	private transient CMPS03 cmps;

	/** The lcd. */
	private transient LCD03 lcd;

	/** The lights. */
	private transient DrivingLights lights;

	/** The robot. */
	private transient Robot robot;

	/** The srf. */
	private transient SRF08 srf;

	/** The state. */
	private transient TimedMultiState state;

	private long wait;

	private RNControl rncontrol;

	private int radarPos = 0;

	private int increment = 45;

	private M256 m256;

	private BehaviorController controller;
	/**
	 * Instantiates a new test.
	 * 
	 * @throws IOException
	 *             Signals that an I/O exception has occurred.
	 */
	public Test() throws IOException {
		super();
	}

	/*
	 * (non-Javadoc)
	 * @see
	 * tc.de.krawallmann.SimpleRoboBridgeApplication#init(tc.de.krawallmann.
	 * robobridge.RoboBridge)
	 */
	@SuppressWarnings({
			"rawtypes", "unchecked"
	})
	@Override
	public void init(final RoboBridge<? extends AbstractRemoteCommunicationDevice> bridge) {
		robot = bridge.getRobot();

		base = new RP6Base(robot, 1);
		base.addSensorListener(this);
		robot.addDevice(base);
		
		rncontrol = new RNControl(robot, 6);
		robot.addDevice(rncontrol);
		rncontrol.addSensorListener(this);

		srf = new SRF08OnServo(rncontrol.getServo(), robot, 2);
		srf.addSensorListener(this);
		robot.addDevice(srf);

		cmps = new CMPS03(robot, 3);
		robot.addDevice(cmps);

		lcd = new LCD03(robot, 4);
		robot.addDevice(lcd);

		lights = new DrivingLights(robot, 5);
		robot.addDevice(lights);

		m256=new M256(robot, 7);
		robot.addDevice(m256);
		
		controller=new BehaviorController(base);
		controller.addBehavior(new EscapeBehavior(robot,  base,1));
		controller.addBehavior(new CruiseBehavior(robot, base, 2));
		
		bridge.getCommunicationInterface().addConnectionListener(new ConnectionListener() {

			@Override
			public void onConnect(AbstractRemoteCommunicationDevice device) {
				if(device instanceof WLANRemoteDevice){
					((WLANRemoteDevice)device).addRSSIListener(m256);
				}
			}

			@Override
			public void onDisconnect(AbstractRemoteCommunicationDevice device, int reason) {
				if(device instanceof WLANRemoteDevice){
					((WLANRemoteDevice)device).removeRSSIListener(m256);
				}
			}


		});
		state = new TimedMultiState(4, 4000);
		state.addStateListener(new MultiStateListener() {

			@Override
			public void onStateChanged(final int state) {
				switch (state) {
					case 0: {
						lights.setReverseLightOn(true);
						lights.setWarnBlinking(false);
						lights.setBlinkingLeft(false);
						lights.setBlinkingRight(false);
						base.setLeds(255);
						break;
					}
					case 1: {
						lights.setBlinkingLeft(true);
						break;
					}
					case 2: {
						lights.setReverseLightOn(false);
						lights.setBlinkingLeft(false);
						lights.setBlinkingRight(true);
						base.setLeds(0);
						break;
					}
					case 3: {
						lights.setWarnBlinking(true);
						break;
					}
					default: {

					}
				}
			}
		});
	}

	/*
	 * (non-Javadoc)
	 * @see
	 * tc.de.krawallmann.SimpleRoboBridgeApplication#onDeviceDiscovered(tc.de
	 * .krawallmann.robobridge.communication.lan.wlan.WLANRemoteDevice)
	 */
	@Override
	public void onDeviceDiscovered(final WLANRemoteDevice device) {
		super.onDeviceDiscovered(device);
		/*
		 * if (!bridge.isConnected()) { try { bridge.connect(device);
		 * bridge.start(); } catch (final IOException e) { e.printStackTrace();
		 * } }
		 */
	}

	/*
	 * (non-Javadoc)
	 * @see tc.de.krawallmann.SimpleRoboBridgeApplication#onFirstLoop()
	 */
	@Override
	public void onFirstLoop() {
		lcd.setText(1, "####################");
		lcd.setText(2, "#RoboBridge Project#");
		lcd.setText(3, "#    PC::ONLINE    #");
		lcd.setText(4, "####################");

		base.setLeds(255);
		lights.setHeadlightsOn(true);
		lights.setWarnBlinking(false);
		bridge.setSensorUpdateInterval(200);
		rncontrol.getServo().setServoPosition(45);
	}

	@Override
	public void onSensorValueChanged(final AbstractDevice sensor, final String propertyName,
			final Object oldValue, final Object newValue) {
		if (!oldValue.equals(newValue) && propertyName.equals(RP6Base.MOVEMENT_COMPLETE)) {
			wait = System.currentTimeMillis();
			return;
		}
		//FIXME: Stimmt die Servo-Pos auch??
		if(propertyName.equals(SRF08.SRF08_DISTANCE)){
			//Measurement ready, move the servo to the next position!
			if (radarPos == 90) {
				increment = -30;
			} else if (radarPos == -90) {
				increment = 30;
			}
			radarPos += increment;
			rncontrol.getServo().setServoPosition(radarPos);
		}
	}

	/*
	 * (non-Javadoc)
	 * @see tc.de.krawallmann.SimpleRoboBridgeApplication#update()
	 */
	@Override
	public void update() {
		state.update();
		/*if (base.isMovementComplete()) {
			if (System.currentTimeMillis() - wait > 2000) {
				base.moveDistance(100, Direction.FWD, 50);
			}
		}*/
		controller.update();
		try {
			Thread.sleep(1);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

}
